//////////////////////////////
// DFRobot SEN0014
class SharpSensor
{
  int _iInfraPin;
  unsigned int _iRefreshRate;
  unsigned long _ulLastTime;
  float _fLastValue;
  float _fAccValue;
  int _iAccCount, _iMaxAccCount;
  float (*_pFuncSensor)(const int);
  float _fMedianFilter[3];
  
  public:
  SharpSensor(const int iInfraPin, const unsigned int iRefreshRate, float (*pFuncSensor)(const int), const int iMaxAccCount)
  {
    _iInfraPin = iInfraPin;
    _iRefreshRate = iRefreshRate;
    _ulLastTime = 0;
    _fLastValue = 0.0f;
    _fAccValue = 0.0f;    
    _iAccCount = 0;
    _iMaxAccCount = iMaxAccCount;
    _pFuncSensor = pFuncSensor;
    _fMedianFilter[0] = _fMedianFilter[1] = _fMedianFilter[2] = -1.0f;
  }
  
  void start()
  {
      pinMode(_iInfraPin, INPUT);
      _ulLastTime = millis();
  }
  
  void update()
  {
    unsigned long ulTime = millis();
  
    if(_ulLastTime == 0)
    {
      _ulLastTime = ulTime;
    }
    // reach _iRefreshRate ?
    else if((_ulLastTime - ulTime) > _iRefreshRate)
    {
      float fDistance = _pFuncSensor(_iInfraPin);
      if(fDistance>0)
      {
        _fAccValue += fDistance;
        _iAccCount++;
        if(_iAccCount>_iMaxAccCount)
        {
          _fLastValue = medianFilter(_fAccValue/(float)_iAccCount);
          _fAccValue = 0;
          _iAccCount = 0;
        }
      }
      
      _ulLastTime = ulTime;
    }
  }
  
  float getDistanceInCm()
  {
    return _fLastValue;
  }
  
  private:
  float medianFilter(const float fValue)
  {
     _fMedianFilter[0] = _fMedianFilter[1];
     _fMedianFilter[1] = _fMedianFilter[2];
     _fMedianFilter[2] = fValue;

     float fMiddle;

     if ((_fMedianFilter[0] <= _fMedianFilter[1]) && (_fMedianFilter[0] <= _fMedianFilter[2]))
     {
       fMiddle = (_fMedianFilter[1] <= _fMedianFilter[2]) ? _fMedianFilter[1] : _fMedianFilter[2];
     }
     else if ((_fMedianFilter[1] <= _fMedianFilter[0]) && (_fMedianFilter[1] <= _fMedianFilter[2]))
     {
       fMiddle = (_fMedianFilter[0] <= _fMedianFilter[2]) ? _fMedianFilter[0] : _fMedianFilter[2];
     }
     else
     {
       fMiddle = (_fMedianFilter[0] <= _fMedianFilter[1]) ? _fMedianFilter[0] : _fMedianFilter[1];
     }
     return fMiddle;
  }
};

SharpSensor _gp2y0a02y(0, 55, oneshot_get_cm_range_sensor_gp2ya02y, 5);
SharpSensor _gp2y0a710(2, 30, oneshot_get_cm_range_sensor_gp2ya710, 5);

void setup()
{
  Serial.begin(9600);
  Serial.println("Start!");
  _gp2y0a02y.start();
  _gp2y0a710.start();
}

unsigned long ulDisplayTime = 0;
void loop()
{
  unsigned long ulCurrentTime = millis();
  
  _gp2y0a02y.update();
  _gp2y0a710.update();
  
  if((ulCurrentTime - ulDisplayTime)>500)
  {
    float fDistance1 = _gp2y0a02y.getDistanceInCm();
    float fDistance2 = _gp2y0a710.getDistanceInCm();
    
    //if(false)
    {
      if(fDistance1>25 && fDistance1<145)
      {
        Serial.print("GP2Y0A02Y (");
        Serial.print(fDistance1);
        Serial.print(") ");
      }
      else
      {
        fDistance1 = -1.0f;
      }
    
      if(fDistance2>110 && fDistance2<500)
      {
        Serial.print("GP2Y0A710 (");
        Serial.print(fDistance2);
        Serial.print(") ");
      }
      else
      {
          fDistance2 = -1.0f;
      }
    }

    Serial.print("Distance = ");
    if(fDistance1 < 0.0f && fDistance2 < 0.0f)
    {
      Serial.print("<OfR>");
    }
    else if(fDistance1 < 0.0f || fDistance1>140)
    {
      Serial.print(fDistance2);
    }
    else
    {
      Serial.print(fDistance1);
    }
    Serial.println(" cm");


    ulDisplayTime = ulCurrentTime;
  }
}
